import asyncio
import logging
from fastapiApp.models.agvs import *
from fastapiApp.models.orders import *
from fastapiApp.models.map import *
from fastapiApp.schemas.agvs_scheams import *
from fastapiApp.models.base import *
import os
import random
import math
import asyncio
import heapq


# 使用统一的日志配置
from fastapiApp.setting.logger_config import get_simulation_logger

logger = get_simulation_logger()

async def async_simulation_clients():
    
    while 1:
        await asyncio.sleep(1)
        # await demo_test()

        try:
            # 使用prefetch_related预加载关联的站点信息，减少数据库查询次数
            # 仅查询运行中的AGV
            query = await AGV_position_Mo.filter(simulation = 1, status = AGVStatus.RUNNING).prefetch_related('node_to_id', 'next_node', 'current_node')
            
            # 遍历查询结果，更新AGV状态和位置
            for role in query:
                """ 没有下一个目标点 重新计算路径                """
                if role.node_to_id is None:
                    continue
                if role.current_node == role.node_to_id:
                    continue
                if role.next_node is None:
                    path = await get_shortest_path(role.current_node.id, role.node_to_id.id)
                    if path and len(path) > 1:
                        role.next_node =  await Map_Node_Mo.get(id = path[1])
                        await role.save()
                        continue

                end_x = role.next_node.x if role.next_node else None
                end_y = role.next_node.y if role.next_node else None


                """ 计算距离 过小时直接到达目标点，不继续移动 状态变更为IDLE 
                移动过程中到达莫一个节点 重新计算路径
                到达目标点 状态变更为IDLE 下一个目标点设为None
                找不到目标点 状态变更为WAITING 重新计算路径
                    """
                distance = math.sqrt((end_x - role.x)**2 + (end_y - role.y)**2)
                if(distance <= role.speed):
                    role.x = end_x
                    role.y = end_y
                    
                    role.current_node = role.next_node
                    # 到达目标点 状态变更为IDLE 下一个目标点设为None
                    if role.next_node == role.node_to_id:
                        role.status = AGVStatus.IDLE
                        role.next_node = None
                    # 未到达目标点 状态变更为WAITING
                    else:
                        role.status = AGVStatus.WAITING
                        # 重新计算路径
                        path = await get_shortest_path(role.current_node.id, role.node_to_id.id)
                        logger.info(f"AGV {role.car_index} 重新计算路径: {path}")
                        if path:
                            role.next_node =  await Map_Node_Mo.get(id = path[1])
                            role.status = AGVStatus.RUNNING
                        else:
                            role.next_node = None

                    await role.save()
                    continue

                # 计算角度            
                angle = math.degrees(math.atan2(end_y - role.y, end_x - role.x))
                if(angle < 0): angle = angle + 360
                role.angle = angle
                # 将角度转换为弧度后计算sin和cos
                radian_angle = math.radians(angle)
                role.x = role.x + math.cos(radian_angle) * role.speed
                role.y = role.y + math.sin(radian_angle) * role.speed

                await role.save()
        
        except Exception as e:
            logger.error(f"AGV位置更新失败: {e}")

        # 未处理订单 寻找附近最近的节点 并分配给AGV
        query = await Order_Mo.filter(status = OrderStatus.PENDING).prefetch_related('order_station_from', 'order_station_to')

        # 遍历查询结果，更新订单状态
        for order in query:
            print(order.order_station_from.id, order.order_station_to.id)
            agv = await AGV_position_Mo.filter(car_index = order.car_index, simulation = 1).first()
            if not agv or agv.status != AGVStatus.IDLE:
                print("AGV不存在或正在执行其他任务")
                continue
            if agv.status == AGVStatus.IDLE:
                order.status = OrderStatus.PROCESSING
                agv.status = AGVStatus.RUNNING
                agv.node_to_id = await find_nearest_node(agv.id)
                order.current_node = order.order_station_from
                order.next_node = await find_nearest_node(agv.id)
                await agv.save()
                await order.save()

        # 处理中订单 寻找附近最近的节点 并分配给AGV
        query = await Order_Mo.filter(status = OrderStatus.PROCESSING).prefetch_related('order_station_from', 'order_station_to')

        for order in query:
            agv = await AGV_position_Mo.filter(car_index = order.car_index, simulation = 1).first()
            if not agv or agv.status != AGVStatus.WAITING:
                print("AGV不存在或正在执行其他任务")
                continue
            if agv.status == AGVStatus.WAITING:
                agv.node_to_id = await find_nearest_station_node(agv.id)
                agv.status = AGVStatus.RUNNING
                await agv.save()
                await order.save()

# 寻找距离AGV当前位置最近的节点
async def find_nearest_node(id: int):
    """
    寻找最近的节点
    :param id: AGV索引
    :return: 最近的节点对象
    """
    agv = await AGV_position_Mo.get(id = id)
    node_list = await Map_Node_Mo.filter(type = StationType.NODE)
    if not node_list:
        return None

    distance = None
    nearest_node = None
    for node in node_list:
        distance_temp = math.sqrt((node.x - agv.x)**2 + (node.y - agv.y)**2)
        if distance is None or distance_temp < distance:
            distance = distance_temp
            nearest_node = node           

    return nearest_node

# 寻找离站点最近的节点
async def find_nearest_station_node(id: int):
    """
    寻找离站点最近的节点
    :param id: 站点索引
    :return: 最近的节点对象
    """
    station = await Map_Node_Mo.get(id = id)
    node_list = await Map_Node_Mo.filter(type = StationType.NODE)
    if not node_list:
        return None

    distance = None
    nearest_node = None
    for station_node in node_list:
        distance_temp = math.sqrt((station_node.x - station.x)**2 + (station_node.y - station.y)**2)
        if distance is None or distance_temp < distance:
            distance = distance_temp
            nearest_node = station_node           

    return nearest_node

"""将边列表转换为邻接矩阵"""  
def convert_to_adjacency_matrix(edges):    
    # 找到最大的节点ID以确定矩阵大小    
    max_node = max(        
        max(edge['node_from_id'], edge['node_to_id'])         
        for edge in edges
    )        

    # 初始化邻接矩阵，使用无穷大表示没有直接连接    
    INF = float('infinity')    
    matrix = [[INF] * (max_node + 1) for _ in range(max_node + 1)]

    # 对角线元素（节点到自身的距离）为0    
    for i in range(max_node + 1):        
        matrix[i][i] = 0  

    # 填充边的距离    
    for edge in edges:        
        from_node = edge['node_from_id']        
        to_node = edge['node_to_id']        
        distance = edge['distance']        
        matrix[from_node][to_node] = distance        
    return matrix, max_node

""" 使用邻接矩阵实现Dijkstra最短路径算法    
    参数:
    matrix: 邻接矩阵
    max_node: 最大节点ID
    start: 起始节点    
    返回:
    distances: 从起始节点到各个节点的最短距离
    predecessors: 用于重建最短路径的前驱节点字典
    """  
def dijkstra(matrix, max_node, start):    
  
    # 初始化距离数组，所有节点的距离先设为无穷大    
    INF = float('infinity')    
    distances = [INF] * (max_node + 1)  

    # 起始节点到自身的距离为0    
    distances[start] = 0        

    # 前驱节点数组，用于记录路径    
    predecessors = [None] * (max_node + 1)        

    # 使用优先队列（堆）来存储需要处理的节点    
    # 堆中的元素是 (距离, 节点)    
    priority_queue = [(0, start)]        
    while priority_queue:        
        # 取出当前距离最小的节点        
        current_distance, current_node = heapq.heappop(priority_queue)  

        # 如果当前距离大于已知最短距离，则跳过        
        if current_distance > distances[current_node]:            
            continue         

        # 遍历所有可能的邻居节点        
        for neighbor in range(max_node + 1):            
            # 检查是否有连接            
            if matrix[current_node][neighbor] != INF:                
                distance = current_distance + matrix[current_node][neighbor]      

                # 如果找到更短的路径，则更新                
                if distance < distances[neighbor]:                    
                    # 更新距离                    
                    distances[neighbor] = distance                    
                    # 更新前驱节点                    
                    predecessors[neighbor] = current_node                    
                    # 将新的距离和节点加入优先队列                    
                    heapq.heappush(priority_queue, (distance, neighbor))        
    return distances, predecessors

"""根据前驱节点数组重建从start到end的最短路径"""
def reconstruct_path(predecessors, start, end):    
        
    path = []    
    current_node = end        
    # 如果终点不可达，返回空路径    
    if predecessors[current_node] is None and start != end:        
        return None        
        # 从终点回溯到起点    
    while current_node is not None:        
        path.append(current_node)        
        current_node = predecessors[current_node]        
        # 反转路径，得到从起点到终点的路径    
    path.reverse()     

    return path

"""计算从start_node到end_node的最短路径"""
async def get_shortest_path(start_node, end_node):
    # 从数据库中获取所有边
    query = await Map_Edge_Mo.all()
    edges = [ await data.to_dict() for data in query ]

    # 转换为邻接矩阵
    adjacency_matrix, max_node = convert_to_adjacency_matrix(edges)
    
    # 计算最短路径
    distances, predecessors = dijkstra(adjacency_matrix, max_node, start_node)
    
    # 重建路径
    path = reconstruct_path(predecessors, start_node, end_node)
    
    return path


async def demo_test():

    query = await Map_Edge_Mo.all()
    edges = [
            await data.to_dict() for data in query
        ]
    # print(edges)
    
    # 转换为邻接矩阵
    adjacency_matrix, max_node = convert_to_adjacency_matrix(edges)
    
    # 打印邻接矩阵（可选，用于验证）
    # print("邻接矩阵:")
    # for row in range(1, max_node + 1):
    #     print(f"节点 {row}: {adjacency_matrix[row][1:max_node+1]}")
    
    # 从节点1开始计算最短路径
    start_node = 6
    distances, predecessors = dijkstra(adjacency_matrix, max_node, start_node)
    
    # 打印结果
    # print(f"\n从节点 {start_node} 到所有其他节点的最短距离:")
    # for node in range(1, max_node + 1):
    #     print(f"到 {node} 的距离: {distances[node]}")
    
    # 重建并打印从1到6的最短路径
    end_node = 13
    path = reconstruct_path(predecessors, start_node, end_node)
    print(f"\n从 {start_node} 到 {end_node} 的最短路径: {path}")
    print(f"路径总长度: {distances[end_node]}")